#include <SnakeLib/Problem.h>

#include "MySolver.h"

#include <vector>
#include <queue>

using namespace snake;

EDirection MySolver::Solve(const snake::Problem &problem)
{
  int move_inf = 10000000;
  size_t height = problem.GetBoard().GetHeight();
  size_t width = problem.GetBoard().GetWidth();
  std::vector<std::vector<int>> matrix(height, std::vector<int>(width, move_inf));

  std::queue<std::pair<int, int>> q;
  for (size_t i = 0; i < height; ++i)
    {
    for (size_t j = 0; j < width; ++j)
      {
      if (problem.GetBoard().GetCell(j, i) == CELL_FOOD)
        {
        matrix[i][j] = 0;
        q.push(std::make_pair(i, j));
        }
      }
    }
  while (q.size())
    {
    auto cur_pos = q.front();
    q.pop();
    auto next_pos=cur_pos;
    next_pos.first--;
    auto next_cell=problem.GetBoard().GetCell(next_pos.second,next_pos.first);
    if(next_cell==CELL_EMPTY || next_cell==CELL_FOOD || next_cell==CELL_POO)
      if(matrix[next_pos.first][next_pos.second]==move_inf)
        {
          matrix[next_pos.first][next_pos.second]=matrix[cur_pos.first][cur_pos.second]+1;
          q.push(next_pos);
        }
    
    next_pos=cur_pos;
    next_pos.first++;
    next_cell=problem.GetBoard().GetCell(next_pos.second,next_pos.first);
    if(next_cell==CELL_EMPTY || next_cell==CELL_FOOD || next_cell==CELL_POO)
      if(matrix[next_pos.first][next_pos.second]==move_inf)
        {
          matrix[next_pos.first][next_pos.second]=matrix[cur_pos.first][cur_pos.second]+1;
          q.push(next_pos);
        }
  
    next_pos=cur_pos;
    next_pos.second--;
    next_cell=problem.GetBoard().GetCell(next_pos.second,next_pos.first);
    if(next_cell==CELL_EMPTY || next_cell==CELL_FOOD || next_cell==CELL_POO)
      if(matrix[next_pos.first][next_pos.second]==move_inf)
        {
          matrix[next_pos.first][next_pos.second]=matrix[cur_pos.first][cur_pos.second]+1;
          q.push(next_pos);
        }
    
    next_pos=cur_pos;
    next_pos.second++;
    next_cell=problem.GetBoard().GetCell(next_pos.second,next_pos.first);
    if(next_cell==CELL_EMPTY || next_cell==CELL_FOOD || next_cell==CELL_POO)
      if(matrix[next_pos.first][next_pos.second]==move_inf)
        {
          matrix[next_pos.first][next_pos.second]=matrix[cur_pos.first][cur_pos.second]+1;
          q.push(next_pos);
        }
    }
      
  auto min_value=10000000;
  EDirection res_direction;
  auto position=GetHeadPosition(problem);
  if(matrix[position.first][position.second-1]<min_value)
    {
    res_direction=LEFT;
    min_value=matrix[position.first][position.second-1];
    }
  if(matrix[position.first][position.second+1]<min_value)
    {
    res_direction=RIGHT;
    min_value=matrix[position.first][position.second+1];
    }
  if(matrix[position.first-1][position.second]<min_value)
    {
    res_direction=UP;
    min_value=matrix[position.first-1][position.second];
    }
  if(matrix[position.first+1][position.second]<min_value)
    {
    res_direction=DOWN;
    min_value=matrix[position.first+1][position.second];
    }
  return res_direction;
}
 
std::pair<int, int> MySolver::GetHeadPosition(const snake::Problem &problem)
  {
  for (size_t i = 0; i < problem.GetBoard().GetHeight(); ++i)
    {
    for (size_t j = 0; j < problem.GetBoard().GetWidth(); ++j)
      {
      auto ret = problem.GetBoard().GetCell(j, i);
      if (ret == CELL_SNAKE_HEAD_DOWN ||
          ret == CELL_SNAKE_HEAD_UP ||
          ret == CELL_SNAKE_HEAD_LEFT ||
          ret == CELL_SNAKE_HEAD_RIGHT)
        {
        return std::make_pair(i, j);
        }
      }
    }
  return std::make_pair(-1, -1);
  }

EDirection MySolver::GetCurrentDirection(const Problem &problem)
  {
    auto position=GetHeadPosition(problem); 
    if(problem.GetBoard().GetCell(position.second,position.first)==CELL_SNAKE_HEAD_UP)
      return UP;
    if(problem.GetBoard().GetCell(position.second,position.first)==CELL_SNAKE_HEAD_LEFT)
      return LEFT;
    if(problem.GetBoard().GetCell(position.second,position.first)==CELL_SNAKE_HEAD_DOWN)
      return DOWN;
    if(problem.GetBoard().GetCell(position.second,position.first)==CELL_SNAKE_HEAD_RIGHT)
      return RIGHT;
    throw std::exception();
  }


bool MySolver::CheckForCrash(const Problem& problem)
  {
  auto position = GetHeadPosition(problem);
  auto direction = GetCurrentDirection(problem);
  if (direction == UP)
    {
    auto nextCell = problem.GetBoard().GetCell(position.second, position.first - 1);
    if (nextCell == CELL_EMPTY || nextCell == CELL_FOOD)
      {
      return false;
      }
    }
  if (direction == DOWN)
    {
    auto nextCell = problem.GetBoard().GetCell(position.second, position.first + 1);
    if (nextCell == CELL_EMPTY || nextCell == CELL_FOOD)
      {
      return false;
      }
    }
  if (direction == LEFT)
    {
    auto nextCell = problem.GetBoard().GetCell(position.second - 1, position.first);
    if (nextCell == CELL_EMPTY || nextCell == CELL_FOOD)
      {
      return false;
      }
    }
  if (direction == RIGHT)
    {
    auto nextCell = problem.GetBoard().GetCell(position.second + 1, position.first);
    if (nextCell == CELL_EMPTY || nextCell == CELL_FOOD || nextCell == CELL_POO)
      {
      return false;
      }
    }
  return true;
  }